int trim[MAXSERVO] = {150,150,150,-50,250,-150,0,0};
//int trim[MAXSERVO] = {0,0,0,-200,100,-300,-150,-150};

void SetServos(int ipos[]){
  for (int i = 0; i< MAXSERVO; i++){
    int pos = ipos[i];
    if (pos < MINPULSE){
      pos = MINPULSE;
    }
    if (pos > MAXPULSE){
      pos = MAXPULSE;
    }
    pos = pos + trim[i];
    Serial3.write(0x80);
    Serial3.write(0x01);
    Serial3.write(0x04);
    Serial3.write(i);
    Serial3.write((pos >> 7) & 0x7F);
    Serial3.write(pos & 0x7F);
  }
}

